//////////////////////////////////////////////////////////////////////
//
//	ZETeam source code 
//	Copyright (c) ZETeam 2011-..
//	
//	File: CVector.cpp
//  Description: Vector classes. Based on Id Software source code.
// 
//	History:
//	- February 26, 2011: Created by Max Streltsov
//
//////////////////////////////////////////////////////////////////////

#include "CVector.h"

#include "prolib.h"

#include "CAngles.h"
#include "CMatrix.h"
#include "CRotation.h"
#include "CQuat.h"

#pragma hdrstop

CVec2 vec2_origin( 0.0f, 0.0f );
CVec3 vec3_origin( 0.0f, 0.0f, 0.0f );
CVec4 vec4_origin( 0.0f, 0.0f, 0.0f, 0.0f );
CVec5 vec5_origin( 0.0f, 0.0f, 0.0f, 0.0f, 0.0f );
CVec6 vec6_origin( 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f );
CVec6 vec6_infinity( CZMath::INFINITY, CZMath::INFINITY, CZMath::INFINITY, CZMath::INFINITY, CZMath::INFINITY, CZMath::INFINITY );


//===============================================================
//
//	CVec2
//
//===============================================================

/*
=============
CVec2::ToString
=============
*/
//const char *CVec2::toString( int precision ) const {
//	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
//}

/*
=============
Lerp

Linearly inperpolates one vector to another.
=============
*/
void CVec2::lerp( const CVec2 &v1, const CVec2 &v2, const float l ) {
	if ( l <= 0.0f ) {
		(*this) = v1;
	} else if ( l >= 1.0f ) {
		(*this) = v2;
	} else {
		(*this) = v1 + l * ( v2 - v1 );
	}
}


//===============================================================
//
//	CVec3
//
//===============================================================

/*
=============
CVec3::ToYaw
=============
*/
float CVec3::toYaw( void ) const {
	float yaw;
	
	if ( ( y == 0.0f ) && ( x == 0.0f ) ) {
		yaw = 0.0f;
	} else {
		yaw = RAD2DEG( atan2( y, x ) );
		if ( yaw < 0.0f ) {
			yaw += 360.0f;
		}
	}

	void * p = (void *) new char[16];
	delete[] p;

	return yaw;
}

/*
=============
CVec3::ToPitch
=============
*/
float CVec3::toPitch( void ) const {
	float	forward;
	float	pitch;
	
	if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
		if ( z > 0.0f ) {
			pitch = 90.0f;
		} else {
			pitch = 270.0f;
		}
	} else {
		forward = ( float )CZMath::Sqrt( x * x + y * y );
		pitch = RAD2DEG( atan2( z, forward ) );
		if ( pitch < 0.0f ) {
			pitch += 360.0f;
		}
	}

	return pitch;
}

/*
=============
CVec3::ToAngles
=============
*/
CAngles CVec3::toAngles( void ) const {
	float forward;
	float yaw;
	float pitch;
	
	if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
		yaw = 0.0f;
		if ( z > 0.0f ) {
			pitch = 90.0f;
		} else {
			pitch = 270.0f;
		}
	} else {
		yaw = RAD2DEG( atan2( y, x ) );
		if ( yaw < 0.0f ) {
			yaw += 360.0f;
		}

		forward = ( float )CZMath::Sqrt( x * x + y * y );
		pitch = RAD2DEG( atan2( z, forward ) );
		if ( pitch < 0.0f ) {
			pitch += 360.0f;
		}
	}

	return CAngles( -pitch, yaw, 0.0f );
}

/*
=============
CVec3::ToPolar
=============
*/
CPolar3 CVec3::toPolar( void ) const {
	float forward;
	float yaw;
	float pitch;
	
	if ( ( x == 0.0f ) && ( y == 0.0f ) ) {
		yaw = 0.0f;
		if ( z > 0.0f ) {
			pitch = 90.0f;
		} else {
			pitch = 270.0f;
		}
	} else {
		yaw = RAD2DEG( atan2( y, x ) );
		if ( yaw < 0.0f ) {
			yaw += 360.0f;
		}

		forward = ( float )CZMath::Sqrt( x * x + y * y );
		pitch = RAD2DEG( atan2( z, forward ) );
		if ( pitch < 0.0f ) {
			pitch += 360.0f;
		}
	}
	return CPolar3( CZMath::Sqrt( x * x + y * y + z * z ), yaw, -pitch );
}

/*
=============
CVec3::ToMat3
=============
*/
CMat3 CVec3::toMat3( void ) const {
	CMat3	mat;
	float	d;

	mat[0] = *this;
	d = x * x + y * y;
	if ( !d ) {
		mat[1][0] = 1.0f;
		mat[1][1] = 0.0f;
		mat[1][2] = 0.0f;
	} else {
		d = CZMath::InvSqrt( d );
		mat[1][0] = -y * d;
		mat[1][1] = x * d;
		mat[1][2] = 0.0f;
	}
	mat[2] = cross( mat[1] );

	return mat;
}

/*
=============
CVec3::ToString
=============
*/
//const char *CVec3::toString( int precision ) const {
//	return CStr::FloatArrayToString( toFloatPtr(), getDimension(), precision );
//}

/*
=============
Lerp

Linearly inperpolates one vector to another.
=============
*/
void CVec3::lerp( const CVec3 &v1, const CVec3 &v2, const float l ) {
	if ( l <= 0.0f ) {
		(*this) = v1;
	} else if ( l >= 1.0f ) {
		(*this) = v2;
	} else {
		(*this) = v1 + l * ( v2 - v1 );
	}
}

/*
=============
SLerp

Spherical linear interpolation from v1 to v2.
Vectors are expected to be normalized.
=============
*/
#define LERP_DELTA 1e-6

void CVec3::sLerp( const CVec3 &v1, const CVec3 &v2, const float t ) {
	float omega, cosom, sinom, scale0, scale1;

	if ( t <= 0.0f ) {
		(*this) = v1;
		return;
	} else if ( t >= 1.0f ) {
		(*this) = v2;
		return;
	}

	cosom = v1 * v2;
	if ( ( 1.0f - cosom ) > LERP_DELTA ) {
		omega = acos( cosom );
		sinom = sin( omega );
		scale0 = sin( ( 1.0f - t ) * omega ) / sinom;
		scale1 = sin( t * omega ) / sinom;
	} else {
		scale0 = 1.0f - t;
		scale1 = t;
	}

	(*this) = ( v1 * scale0 + v2 * scale1 );
}

/*
=============
ProjectSelfOntoSphere

Projects the z component onto a sphere.
=============
*/
void CVec3::projectSelfOntoSphere( const float radius ) {
	float rsqr = radius * radius;
	float len = length();
	if ( len  < rsqr * 0.5f ) {
		z = sqrt( rsqr - len );
	} else {
		z = rsqr / ( 2.0f * sqrt( len ) );
	}
}



//===============================================================
//
//	CVec4
//
//===============================================================

/*
=============
CVec4::ToString
=============
*/
//const char *CVec4::ToString( int precision ) const {
//	return CStr::floatArrayToString( toFloatPtr(), getDimension(), precision );
//}

/*
=============
Lerp

Linearly inperpolates one vector to another.
=============
*/
void CVec4::lerp( const CVec4 &v1, const CVec4 &v2, const float l ) {
	if ( l <= 0.0f ) {
		(*this) = v1;
	} else if ( l >= 1.0f ) {
		(*this) = v2;
	} else {
		(*this) = v1 + l * ( v2 - v1 );
	}
}


//===============================================================
//
//	CVec5
//
//===============================================================

/*
=============
CVec5::ToString
=============
*/
//const char *CVec5::toString( int precision ) const {
//	return CStr::floatArrayToString( toFloatPtr(), getDimension(), precision );
//}

/*
=============
CVec5::Lerp
=============
*/
void CVec5::lerp( const CVec5 &v1, const CVec5 &v2, const float l ) {
	if ( l <= 0.0f ) {
		(*this) = v1;
	} else if ( l >= 1.0f ) {
		(*this) = v2;
	} else {
		x = v1.x + l * ( v2.x - v1.x );
		y = v1.y + l * ( v2.y - v1.y );
		z = v1.z + l * ( v2.z - v1.z );
		s = v1.s + l * ( v2.s - v1.s );
		t = v1.t + l * ( v2.t - v1.t );
	}
}


//===============================================================
//
//	CVec6
//
//===============================================================

/*
=============
CVec6::ToString
=============
*/
//const char *CVec6::toString( int precision ) const {
//	return CStr::floatArrayToString( toFloatPtr(), getDimension(), precision );
//}


//===============================================================
//
//	CVecX
//
//===============================================================
/*
float	idVecX::temp[VECX_MAX_TEMP+4];
float *	idVecX::tempPtr = (float *) ( ( (int) idVecX::temp + 15 ) & ~15 );
int		idVecX::tempIndex = 0;

=============
idVecX::ToString
=============
const char *idVecX::ToString( int precision ) const {
	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
}
//*/